#!/usr/bin/env python3

"""
系统诊断脚本 - 检查无人机导航系统的运行环境
用于部署前的环境验证和故障排查
"""

import subprocess
import sys
import os
import importlib
import platform
from pathlib import Path
import json

class SystemDiagnostics:
    """系统诊断工具"""
    
    def __init__(self):
        self.results = {
            'system': {},
            'ros2': {},
            'python_packages': {},
            'hardware': {},
            'navigation_system': {},
            'recommendations': []
        }
        
    def run_command(self, command, capture_output=True, timeout=10):
        """运行系统命令"""
        try:
            result = subprocess.run(
                command.split(), 
                capture_output=capture_output, 
                text=True, 
                timeout=timeout
            )
            return result.returncode == 0, result.stdout, result.stderr
        except subprocess.TimeoutExpired:
            return False, "", "命令超时"
        except Exception as e:
            return False, "", str(e)
    
    def check_system_info(self):
        """检查系统基本信息"""
        print("🖥️  检查系统信息...")
        
        self.results['system'] = {
            'platform': platform.platform(),
            'architecture': platform.architecture()[0],
            'python_version': platform.python_version(),
            'cpu_count': os.cpu_count(),
        }
        
        # 检查是否为Jetson设备
        jetson_model = self.detect_jetson_model()
        if jetson_model:
            self.results['system']['jetson_model'] = jetson_model
            print(f"  ✅ 检测到Jetson设备: {jetson_model}")
        else:
            print("  ⚠️  未检测到Jetson设备")
        
        # 检查内存
        try:
            import psutil
            memory = psutil.virtual_memory()
            self.results['system']['memory_total_gb'] = round(memory.total / (1024**3), 2)
            self.results['system']['memory_available_gb'] = round(memory.available / (1024**3), 2)
            
            if memory.total < 6 * (1024**3):  # 少于6GB
                self.results['recommendations'].append("建议使用8GB或更大内存的设备")
                print(f"  ⚠️  内存容量较低: {self.results['system']['memory_total_gb']:.1f}GB")
            else:
                print(f"  ✅ 内存容量充足: {self.results['system']['memory_total_gb']:.1f}GB")
                
        except ImportError:
            self.results['recommendations'].append("需要安装psutil包")
        
    def detect_jetson_model(self):
        """检测Jetson设备型号"""
        model_files = [
            '/sys/firmware/devicetree/base/model',
            '/proc/device-tree/model'
        ]
        
        for model_file in model_files:
            try:
                if os.path.exists(model_file):
                    with open(model_file, 'r') as f:
                        content = f.read().strip().replace('\x00', '')
                        if 'jetson' in content.lower():
                            return content
            except:
                continue
        
        return None
    
    def check_ros2_environment(self):
        """检查ROS2环境"""
        print("🤖 检查ROS2环境...")
        
        # 检查ROS2安装
        ros2_path = '/opt/ros/humble'
        if os.path.exists(ros2_path):
            self.results['ros2']['installed'] = True
            self.results['ros2']['path'] = ros2_path
            print("  ✅ ROS2 Humble已安装")
        else:
            self.results['ros2']['installed'] = False
            self.results['recommendations'].append("需要安装ROS2 Humble")
            print("  ❌ ROS2 Humble未安装")
            return
        
        # 检查ROS2环境变量
        ros_distro = os.environ.get('ROS_DISTRO')
        if ros_distro == 'humble':
            self.results['ros2']['distro'] = ros_distro
            print(f"  ✅ ROS2环境变量正确: {ros_distro}")
        else:
            print("  ⚠️  ROS2环境变量未设置，请运行: source /opt/ros/humble/setup.bash")
            self.results['recommendations'].append("设置ROS2环境变量")
        
        # 检查关键ROS2包
        ros2_packages = [
            'navigation2',
            'nav2_simple_commander',
            'pcl_conversions',
            'tf2_ros'
        ]
        
        missing_packages = []
        for package in ros2_packages:
            success, _, _ = self.run_command(f"ros2 pkg list | grep {package}")
            if success:
                print(f"  ✅ {package}")
            else:
                print(f"  ❌ {package} 缺失")
                missing_packages.append(package)
        
        if missing_packages:
            self.results['ros2']['missing_packages'] = missing_packages
            self.results['recommendations'].append(f"安装缺失的ROS2包: {', '.join(missing_packages)}")
    
    def check_python_packages(self):
        """检查Python依赖包"""
        print("🐍 检查Python依赖包...")
        
        required_packages = {
            'numpy': '数值计算',
            'scipy': '科学计算',
            'opencv-python': '计算机视觉',
            'scikit-learn': '机器学习',
            'psutil': '系统监控',
            'transforms3d': '3D变换',
            'pyserial': '串口通信'
        }
        
        for package, description in required_packages.items():
            try:
                # 尝试导入标准名称
                import_name = package.replace('-', '_').replace('opencv_python', 'cv2')
                importlib.import_module(import_name)
                print(f"  ✅ {package} ({description})")
                self.results['python_packages'][package] = True
            except ImportError:
                print(f"  ❌ {package} ({description}) 缺失")
                self.results['python_packages'][package] = False
                
        # 检查缺失的包
        missing = [pkg for pkg, status in self.results['python_packages'].items() if not status]
        if missing:
            self.results['recommendations'].append(f"安装Python包: pip3 install {' '.join(missing)}")
    
    def check_hardware_status(self):
        """检查硬件状态"""
        print("🔧 检查硬件状态...")
        
        # 检查GPU
        gpu_available, gpu_info, _ = self.run_command("nvidia-smi --query-gpu=name,memory.total --format=csv,noheader")
        if gpu_available:
            self.results['hardware']['gpu'] = True
            self.results['hardware']['gpu_info'] = gpu_info.strip()
            print(f"  ✅ GPU可用: {gpu_info.strip()}")
        else:
            self.results['hardware']['gpu'] = False
            print("  ⚠️  GPU不可用或nvidia-smi未安装")
        
        # 检查传感器设备
        self.check_sensor_devices()
        
        # 检查串口设备
        self.check_serial_devices()
    
    def check_sensor_devices(self):
        """检查传感器设备"""
        # 检查USB设备
        usb_success, usb_output, _ = self.run_command("lsusb")
        if usb_success:
            # 查找Livox设备
            if 'livox' in usb_output.lower():
                print("  ✅ 检测到Livox设备")
                self.results['hardware']['livox_detected'] = True
            else:
                print("  ⚠️  未检测到Livox设备")
                self.results['hardware']['livox_detected'] = False
        
        # 检查网络设备（某些激光雷达使用以太网）
        net_success, net_output, _ = self.run_command("ip link show")
        if net_success:
            interface_count = net_output.count('state UP')
            print(f"  📡 网络接口状态: {interface_count} 个活跃接口")
    
    def check_serial_devices(self):
        """检查串口设备"""
        serial_devices = []
        
        # 检查常见串口设备
        serial_paths = ['/dev/ttyUSB*', '/dev/ttyACM*', '/dev/ttyS*']
        
        for pattern in serial_paths:
            devices = list(Path('/dev').glob(pattern.split('/')[-1]))
            serial_devices.extend([str(d) for d in devices])
        
        if serial_devices:
            self.results['hardware']['serial_devices'] = serial_devices
            print(f"  📱 串口设备: {', '.join(serial_devices)}")
        else:
            print("  ⚠️  未检测到串口设备")
            self.results['hardware']['serial_devices'] = []
    
    def check_navigation_system(self):
        """检查导航系统文件"""
        print("🧭 检查导航系统...")
        
        # 检查工作空间
        ws_path = Path.home() / 'drone_navigation_ws'
        if ws_path.exists():
            self.results['navigation_system']['workspace_exists'] = True
            print(f"  ✅ 工作空间存在: {ws_path}")
            
            # 检查关键文件
            key_files = [
                'src/drone_navigation/drone_navigation/navigation_manager.py',
                'src/drone_navigation/drone_navigation/obstacle_detector.py',
                'src/drone_navigation/drone_navigation/drone_navigator.py',
                'src/drone_navigation/config/navigation_params.yaml'
            ]
            
            missing_files = []
            for file_path in key_files:
                full_path = ws_path / file_path
                if full_path.exists():
                    print(f"  ✅ {file_path}")
                else:
                    print(f"  ❌ {file_path} 缺失")
                    missing_files.append(file_path)
            
            if missing_files:
                self.results['navigation_system']['missing_files'] = missing_files
                
        else:
            self.results['navigation_system']['workspace_exists'] = False
            print("  ❌ 导航工作空间不存在")
            self.results['recommendations'].append("创建并配置导航工作空间")
        
        # 检查point_lio
        point_lio_path = Path.home() / 'point_lio' 
        if point_lio_path.exists():
            print("  ✅ point_lio存在")
            self.results['navigation_system']['point_lio_exists'] = True
        else:
            print("  ⚠️  point_lio不存在，请确认安装路径")
            self.results['navigation_system']['point_lio_exists'] = False
    
    def generate_report(self):
        """生成诊断报告"""
        print("\n" + "="*60)
        print("📋 系统诊断报告")
        print("="*60)
        
        # 系统概览
        print(f"\n🖥️  系统信息:")
        print(f"   平台: {self.results['system'].get('platform', 'Unknown')}")
        print(f"   架构: {self.results['system'].get('architecture', 'Unknown')}")
        print(f"   Python: {self.results['system'].get('python_version', 'Unknown')}")
        print(f"   内存: {self.results['system'].get('memory_total_gb', 'Unknown')}GB")
        
        if 'jetson_model' in self.results['system']:
            print(f"   设备: {self.results['system']['jetson_model']}")
        
        # ROS2状态
        print(f"\n🤖 ROS2状态:")
        if self.results['ros2'].get('installed'):
            print("   ✅ ROS2 Humble已安装")
            if 'missing_packages' in self.results['ros2']:
                print(f"   ⚠️  缺失包: {', '.join(self.results['ros2']['missing_packages'])}")
        else:
            print("   ❌ ROS2未安装")
        
        # Python包状态
        print(f"\n🐍 Python包状态:")
        missing_py_packages = [pkg for pkg, status in self.results['python_packages'].items() if not status]
        if missing_py_packages:
            print(f"   ⚠️  缺失: {', '.join(missing_py_packages)}")
        else:
            print("   ✅ 所有必需包已安装")
        
        # 硬件状态
        print(f"\n🔧 硬件状态:")
        if self.results['hardware'].get('gpu'):
            print(f"   ✅ GPU: {self.results['hardware'].get('gpu_info', 'Available')}")
        else:
            print("   ⚠️  GPU不可用")
            
        if self.results['hardware'].get('livox_detected'):
            print("   ✅ Livox设备已检测")
        else:
            print("   ⚠️  Livox设备未检测")
        
        # 建议
        if self.results['recommendations']:
            print(f"\n💡 建议:")
            for i, rec in enumerate(self.results['recommendations'], 1):
                print(f"   {i}. {rec}")
        else:
            print(f"\n✅ 系统状态良好，无需特殊操作")
        
        print("\n" + "="*60)
    
    def save_report(self, filename='system_diagnostics.json'):
        """保存诊断报告到文件"""
        try:
            with open(filename, 'w', encoding='utf-8') as f:
                json.dump(self.results, f, indent=2, ensure_ascii=False)
            print(f"\n📄 诊断报告已保存到: {filename}")
        except Exception as e:
            print(f"\n❌ 保存报告失败: {e}")
    
    def run_full_diagnostics(self):
        """运行完整诊断"""
        print("🔍 开始系统诊断...\n")
        
        self.check_system_info()
        print()
        
        self.check_ros2_environment()
        print()
        
        self.check_python_packages()
        print()
        
        self.check_hardware_status()
        print()
        
        self.check_navigation_system()
        
        self.generate_report()
        self.save_report()

def main():
    """主函数"""
    if len(sys.argv) > 1 and sys.argv[1] in ['-h', '--help']:
        print("系统诊断工具 - 检查无人机导航系统环境")
        print("用法: python3 system_diagnostics.py")
        print("选项:")
        print("  -h, --help    显示帮助信息")
        return
    
    try:
        diagnostics = SystemDiagnostics()
        diagnostics.run_full_diagnostics()
    except KeyboardInterrupt:
        print("\n\n⏹️  诊断被用户中断")
    except Exception as e:
        print(f"\n❌ 诊断过程出错: {e}")
        import traceback
        traceback.print_exc()

if __name__ == '__main__':
    main() 